/*************************************************************************
 * filename: pushrod_bsp.c
 * Abstract: The scheduler of pushrod
 * describe: The scheduler of pushrod
 * author: 	zhw
 * version: v1.1
 * date: 	2020-05-17
 *************************************************************************/
#include <string.h>
#include "pushrod_cfg.h"
#include "pushrod_bsp.h"

#ifndef TRUE
#define TRUE 1
#define FALSE 0
#endif

#ifndef NULL
#define NULL (void *)0
#endif

#if PUSHROD_MEM
typedef enum {
    PUSHROD_POINT_NORMAL,
    PUSHROD_POINT_BACK,
} pushrod_run_sta_t;

typedef enum {
    PUSHROD_TOP = 0,                        /*<推杆到了顶部*/
    PUSHROD_MIDDLE,                         /*<推杆不在顶部和底部*/
    PUSHROD_BOTTOM                          /*!<推杆在底部*/
} pushrod_range_t;
#endif

typedef enum {
    PUSHROD_SCHE_IDLE = 0x00,
    PUSHROD_SCHE_BUSY,
} pushrod_sche_sta_t;

typedef enum {
    OPEN_BROKEN = 0x1,                      /*!<open方向信号超时*/
    CLOSE_BROKEN = 0x2                      /*!<close方向信号超时*/
} signal_chan_sta_t;

typedef struct  {
    pushrod_sta_t expected;                 /*!<推杆期望的状态，想要控制的状态，主要涉及到了推杆换向问题*/
    pushrod_sta_t status;                   /*!<推杆当前状态*/
    pushrod_u16_t dev;                      /*!<推杆设备号*/
    pushrod_u32_t sche_time;                /*!<换向时间*/
#if PUSHROD_MEM
    struct {
        pushrod_u8_t capture;               /*!<推杆信号补获标志*/
        pushrod_u8_t working_flag;          /*!<信号采集板工作情况*/
        pushrod_u32_t value;                /*!<推杆信号采集值，当前值*/
        pushrod_u32_t last_value;           /*!<推杆信号采集值，上一次*/
        pushrod_s32_t current_position;     /*!<推杆当前位置*/
        pushrod_u32_t max_position;         /*!<推杆最大位置*/
        // ==================================================================
        /// @brief  : average_period 变量说明
        //          1、为了实现动态补偿推杆启动时候的波形丢失
        //          2、引入一个变量记录每一次推杆运行时的平均波形，为下一次推杆工作做参照补偿
        // ==================================================================
        pushrod_u16_t average_period;       /*!<平均波形*/
        pushrod_u16_t real_waveform;        /*!<真实波形，用于计算平均波形,也可以说是稳定波形*/
        pushrod_u16_t learn_len;            /*!<学习波形个数*/
        pushrod_u32_t learn_count;          /*!<学习波形总时长*/
        pushrod_u16_t range_count;         /*!<用于检测信号采集板好坏*/
        // ==================================================================
        /// @brief  : range变量说明
        //              1、为了实现推杆到底部和顶部后，在按打开或缩回时推杆不用再工作，以免造成波形计算错误导致位置不准确
        // ==================================================================
        pushrod_range_t range;              /*!<推杆当前范围*/
        pushrod_run_sta_t run_sta;          /*!<推杆工作模式*/
        pushrod_s32_t point_position;       /*!<当前处理的记忆位置*/
        // ==================================================================
        /// @brief  : 信号模块动态监测实现原理
        //              1、当推杆向一个方向运行超时没有检测波形后就标记此方向为坏的,此时此方向的操作将会被屏蔽，因为有判断到顶检测也是这个方向的。
        //              2、当在往另一个方向运动时也检测到波形超时，就认为信号采集电路坏了(判断依据是两个方向操作波形检测都超时了)
        //              3、只要在定时器捕获中断有数据，就认为信号采集模块是好的。
        //              以上往复动态检测，可以实现信号模块的动态判断，不影响推杆打开和收回操作，但对记忆位置肯定是有影响的。
        // ==================================================================
        pushrod_u8_t signal_chan_sta;       /*!<当前信号通道状态*/
    } memory;
#endif
} pushrod_t;

// ==================================================================
/// @brief  : PORT DECLARATION
// ==================================================================
void Push_Port_Init(void);
void Pushrod_Open(pushrod_u16_t);
void Pushrod_Close(pushrod_u16_t);
void Pushrod_Stop(pushrod_u16_t);
pushrod_u16_t Pushrod_Clk_Get(void);

static pushrod_t pushrod[PUSHROD_NUM];

static void Pushrod_Set(pushrod_u8_t index, pushrod_sta_t sta)
{
    pushrod[index].expected = sta;
}

static pushrod_u8_t Pushrod_Get(pushrod_u16_t dev)
{
    for (int i = 0; i < PUSHROD_NUM; ++i) {
        if (dev & ((pushrod_u16_t)1 << i)) {
            return ((pushrod_u8_t)i);
        }
    }

    return (PUSHROD_NUM);
}

static pushrod_t *Pushrod_Diff(void)
{
    for (int i = 0; i < PUSHROD_NUM; i++) {
        if (pushrod[i].status != pushrod[i].expected) {
            if (pushrod[i].sche_time != 0) {
                continue;
            }
            return (&pushrod[i]);
        }
    }

    return NULL;
}

static void Pushrod_Sche(void)
{
    static pushrod_sche_sta_t pushrod_sche_sta = PUSHROD_SCHE_IDLE;
    static pushrod_t *pushrod_tmp;
    static pushrod_u8_t busy_time;

    #ifndef OS
    pushrod_u16_t tmp_clk = 0;
    static pushrod_u16_t last_clk = 0;

    tmp_clk = Pushrod_Clk_Get();

    if ((pushrod_u16_t)(tmp_clk - last_clk) < 20) {
        return;
    }

    last_clk = tmp_clk;
    #endif   

    switch(pushrod_sche_sta) {

        case PUSHROD_SCHE_IDLE: {

            pushrod_tmp = Pushrod_Diff();

            if (pushrod_tmp != NULL) {
                busy_time = 0;
                if (pushrod_tmp->expected == PUSHROD_OPEN) {
                    if (pushrod_tmp->status == PUSHROD_CLOSE) {
                        Pushrod_Stop(pushrod_tmp->dev);
                        pushrod_tmp->sche_time = SWITCH_TIME;
                        pushrod_sche_sta = PUSHROD_SCHE_BUSY;
                    } else if (pushrod_tmp->status == PUSHROD_STOP) {
                        Pushrod_Open(pushrod_tmp->dev);
                        pushrod_tmp->status = PUSHROD_OPEN;
                        pushrod_sche_sta = PUSHROD_SCHE_BUSY;
                    }
                } else if (pushrod_tmp->expected == PUSHROD_CLOSE) {
                    if (pushrod_tmp->status == PUSHROD_OPEN) {
                        Pushrod_Stop(pushrod_tmp->dev);
                        pushrod_tmp->sche_time = SWITCH_TIME;
                        pushrod_sche_sta = PUSHROD_SCHE_BUSY;
                    } else if (pushrod_tmp->status == PUSHROD_STOP) {
                        Pushrod_Close(pushrod_tmp->dev);
                        pushrod_tmp->status = PUSHROD_CLOSE;
                        pushrod_sche_sta = PUSHROD_SCHE_BUSY;
                    }
                } else {
                    if (pushrod_tmp->sche_time == 0) {
                        Pushrod_Stop(pushrod_tmp->dev);
                        pushrod_tmp->sche_time = SWITCH_TIME;
                    }
                }
            }
            break;
        }

        case PUSHROD_SCHE_BUSY: {
            busy_time++;
            if (busy_time >= SWITCH_TIME) {
                pushrod_sche_sta = PUSHROD_SCHE_IDLE;
            }
            break;
        }

        default:
            break;
    }

    for (int i = 0; i < PUSHROD_NUM; ++i) {
        if (pushrod[i].sche_time != 0) {
            pushrod[i].sche_time--;

            if (pushrod[i].sche_time == 0)
                pushrod[i].status = PUSHROD_STOP;
        }
    }
}

#if PUSHROD_MEM
static void Flag_Clean(void)
{
    for (int i = 0; i < PUSHROD_NUM; i++) {
        pushrod[i].memory.run_sta = PUSHROD_POINT_NORMAL;
        if (!pushrod[i].memory.working_flag) {
            pushrod[i].memory.range = PUSHROD_MIDDLE;
        }
    }
}

static void Pushrod_Scan(void)
{
    #ifndef OS
    pushrod_u16_t scan_clk_tmp = 0;
    static pushrod_u16_t scan_clk = 0;

    scan_clk_tmp = Pushrod_Clk_Get();

    if ((pushrod_u16_t)(scan_clk_tmp - scan_clk) > 5)
        scan_clk = scan_clk_tmp;
    else
        return ;
    #endif

    for (int i = 0; i < PUSHROD_NUM; i++) {
        if (!pushrod[i].memory.working_flag)
            continue;
        pushrod[i].memory.range_count++;
        if (pushrod[i].status == PUSHROD_OPEN) {
            if (pushrod[i].memory.range_count > RANGE_COUNT_VALUE) {
                
                pushrod[i].memory.signal_chan_sta |= OPEN_BROKEN;

                #if (!HOULE_ENABLE)
                // 检测模块异常,向两个方向操作都超时了
                if (pushrod[i].memory.signal_chan_sta == (OPEN_BROKEN | CLOSE_BROKEN)) {
                    pushrod[i].memory.working_flag = FALSE; 
                    pushrod[i].memory.range = PUSHROD_MIDDLE;
                    continue;
                }
                #endif
                Pushrod_Set(i, PUSHROD_STOP);

                pushrod[i].memory.range = PUSHROD_TOP;//!
                pushrod[i].memory.run_sta = PUSHROD_POINT_NORMAL;

                if (pushrod[i].memory.max_position <= 1) {
                    pushrod[i].memory.max_position = pushrod[i].memory.current_position;
                } else {
                    pushrod[i].memory.current_position = pushrod[i].memory.max_position;//!
                }
            }
        } else if (pushrod[i].status == PUSHROD_CLOSE) {
            if (pushrod[i].memory.range_count > RANGE_COUNT_VALUE) {

                pushrod[i].memory.signal_chan_sta |= CLOSE_BROKEN;

                #if (!HOULE_ENABLE)
                // 检测模块异常,向两个方向操作都超时了
                if (pushrod[i].memory.signal_chan_sta == (OPEN_BROKEN | CLOSE_BROKEN)) {
                    pushrod[i].memory.working_flag = FALSE;
                    pushrod[i].memory.range = PUSHROD_MIDDLE;
                    continue;
                }
                #endif

                Pushrod_Set(i, PUSHROD_STOP);

                #if (!HOULE_ENABLE)
                #endif
            
                pushrod[i].memory.range = PUSHROD_BOTTOM;//!
                pushrod[i].memory.current_position = 0;//!
                pushrod[i].memory.learn_count = 0;
                pushrod[i].memory.learn_len = 0;
                pushrod[i].memory.max_position = 0;
                pushrod[i].memory.run_sta = PUSHROD_POINT_NORMAL;
            }
        } else {
            pushrod[i].memory.range_count = 0;
            pushrod[i].memory.capture = FALSE;
        }
    }
}
#endif

void Pushrod_Init(void)
{
    Push_Port_Init();

    for (int i = 0; i < PUSHROD_NUM; i++) {
        pushrod[i].expected = PUSHROD_STOP;
        pushrod[i].status = PUSHROD_STOP;
        pushrod[i].dev = (pushrod_u16_t)((pushrod_u16_t)1 << i);
        pushrod[i].sche_time = 0;
#if PUSHROD_MEM
        pushrod[i].memory.capture = FALSE;
        pushrod[i].memory.working_flag = PUSHROD_MEM;
        pushrod[i].memory.value = 0;
        pushrod[i].memory.last_value = 0;
        pushrod[i].memory.current_position = 0;
        pushrod[i].memory.max_position = 1;
        pushrod[i].memory.average_period = 700;
        pushrod[i].memory.learn_len = 0;
        pushrod[i].memory.learn_count = 0;
        pushrod[i].memory.range_count = 0;
        pushrod[i].memory.range = PUSHROD_MIDDLE;
        pushrod[i].memory.run_sta = PUSHROD_POINT_NORMAL;
        pushrod[i].memory.point_position = 0;
        pushrod[i].memory.signal_chan_sta = 0;
#endif
    }
}

void Pushrod_Process(void)
{
    Pushrod_Sche();
#if PUSHROD_MEM
    Pushrod_Scan();
#endif
}

void Pushrod_Opt(pushrod_u16_t dev, pushrod_sta_t sta)
{
    for (int i = 0; i < PUSHROD_NUM; i++) {
        if (dev & ((pushrod_u16_t)1 << i)) {
            if (sta == PUSHROD_OPEN) {
#if PUSHROD_MEM
                if ((pushrod[i].memory.range != PUSHROD_TOP) || !pushrod[i].memory.working_flag)   
#endif
                    Pushrod_Set(i, sta);
            } else if (sta == PUSHROD_CLOSE){
#if PUSHROD_MEM
                if ((pushrod[i].memory.range != PUSHROD_BOTTOM) || !pushrod[i].memory.working_flag)
#endif
                    Pushrod_Set(i, sta);
            } else {
#if PUSHROD_MEM
                Flag_Clean();
#endif
                Pushrod_Set(i, sta);
            }
        }
    }
}

#if PUSHROD_MEM
void Pushrod_Capture(pushrod_u16_t dev, pushrod_u16_t value)
{
    pushrod_u16_t diff;
    pushrod_u8_t index = 0;

    index = Pushrod_Get(dev);

    if (index >= PUSHROD_NUM)
        return ;

    pushrod[index].memory.signal_chan_sta = 0;
    pushrod[index].memory.range_count = 0;
    // 补获是每两次计算一次时间
    if (pushrod[index].memory.capture == FALSE) {
        pushrod[index].memory.value = value;
        pushrod[index].memory.capture = TRUE;
        pushrod[index].memory.learn_len = 0;
        pushrod[index].memory.learn_count = 0;
        pushrod[index].memory.real_waveform = Pushrod_Clk_Get();
    } else {
        pushrod[index].memory.range = PUSHROD_MIDDLE;
        pushrod[index].memory.last_value = pushrod[index].memory.value;
        pushrod[index].memory.value = value;

        diff = (pushrod_u16_t)(pushrod[index].memory.value - pushrod[index].memory.last_value);

        if (diff > (pushrod_u16_t)(pushrod[index].memory.average_period * 20)) {
            return ;
        }

        #if (!HOULE_ENABLE)
        if (diff >= (pushrod_u16_t)(pushrod[index].memory.average_period * 5)) {
            // 大于平均波形的5倍，需要补偿。5倍这个值是通过测试大概估算的，所以补偿并不是很准确
            if (pushrod[index].status == PUSHROD_OPEN) {
                pushrod[index].memory.current_position += (diff / pushrod[index].memory.average_period );
            } else if (pushrod[index].status == PUSHROD_CLOSE) {
                if ((pushrod[index].memory.current_position - diff / pushrod[index].memory.average_period) > 0) {
                    pushrod[index].memory.current_position -= (diff / pushrod[index].memory.average_period );
                } else {
                    pushrod[index].memory.current_position = 0;
                }
            } else {
                return;
            }
        } else {
            #endif
            if (pushrod[index].status == PUSHROD_OPEN) {
                pushrod[index].memory.current_position++;
            } else if (pushrod[index].status == PUSHROD_CLOSE) {
                if (pushrod[index].memory.current_position) {
                    pushrod[index].memory.current_position--;
                } else {
                    pushrod[index].memory.current_position = 0;
                }
            } else{
                return;
            }
            // 只要有捕获波形输入，之前判定模块是好的
            if(pushrod[index].memory.working_flag != TRUE) {
                pushrod[index].memory.working_flag = TRUE;
            }
            // 推杆刚启动时的波形不平稳，等波形平稳后再算平均波形
            if ((pushrod_u16_t)(Pushrod_Clk_Get() - pushrod[index].memory.real_waveform) > REAL_WAVEFORM_LEN) {
                if (pushrod[index].memory.learn_len < LEARN_LEN) {
                    pushrod[index].memory.learn_count += diff;
                    pushrod[index].memory.learn_len++;

                    if (pushrod[index].memory.learn_len >= LEARN_LEN) {
                        // 只要推杆工作，就计算平均波形，为下一次推杆工作做信号补偿参考
                        pushrod[index].memory.average_period = (pushrod_u16_t)(pushrod[index].memory.learn_count / LEARN_LEN );
                    }
                } else {

                }
            }
        #if (!HOULE_ENABLE)
        }
        #endif

        if (pushrod[index].memory.run_sta == PUSHROD_POINT_BACK) {
            // 判断记忆位置和当前位置是否相等，并执行停止动作
            if ((pushrod[index].status == PUSHROD_OPEN) && pushrod[index].expected == PUSHROD_OPEN){
                if (pushrod[index].memory.current_position >= pushrod[index].memory.point_position) {
                    Pushrod_Set(index, PUSHROD_STOP);
                    pushrod[index].memory.run_sta  = PUSHROD_POINT_NORMAL;
                }
            } else if (pushrod[index].status == PUSHROD_CLOSE && pushrod[index].expected == PUSHROD_CLOSE) {
                if (pushrod[index].memory.current_position <= pushrod[index].memory.point_position) {
                    Pushrod_Set(index, PUSHROD_STOP);
                    pushrod[index].memory.run_sta  = PUSHROD_POINT_NORMAL;
                }
            }
        }
    }
}

void Pushrod_Set_Point(pushrod_s32_t *point)
{
    for (int i = 0; i < PUSHROD_NUM; i++)
       point[i] = pushrod[i].memory.current_position;
}

void Pushrod_Back_Point(pushrod_s32_t *point)
{
    for (int i = 0; i < PUSHROD_NUM; ++i) {
        if (pushrod[i].memory.working_flag == FALSE) {
            continue;
        }
        // 在100个坐标点内不执行记忆返回
        if (point[i] < pushrod[i].memory.current_position - 100) {
            Pushrod_Set(i , PUSHROD_CLOSE); 
        } else if (point[i] > pushrod[i].memory.current_position + 100) {
            Pushrod_Set(i , PUSHROD_OPEN);
        } else {
            continue;
        }

        pushrod[i].memory.run_sta = PUSHROD_POINT_BACK;
        pushrod[i].memory.point_position = point[i];
    }
}

void Pushrod_Set_Info(pushrod_info_t *info)
{
    for (int i = 0; i < PUSHROD_NUM; i++) {
        pushrod[i].memory.current_position = info->current_position[i];
    }
}

void Pushrod_Get_Info(pushrod_info_t *info)
{
    for (int i = 0; i < PUSHROD_NUM; i++) {
        info->current_position[i] = pushrod[i].memory.current_position;
        info->status[i] = pushrod[i].status;
    }
}
#endif
